#include "pid.h"
#include "stdio.h"
#include "math.h"

#include "change.h" //主从机切换
//定义全局变量

_pid pid_speed, pid_speed2;    
_pid pid_location;
_pid pid_angle;

int pid_location_flag = 0;  //是否启用位置环

#define user_pid

#ifdef user_pid


#define user_pid_printf(format, ...)printf( format "\r\n", ##__VA_ARGS__)
#define user_pid_info(format, ...)printf("info:" format "\r\n", ##__VA_ARGS__)
#define user_pid_debug(format, ...)printf("pid_debug:"  format "\r\n", ##__VA_ARGS__)
#define user_pid_error(format, ...)printf("error:"  format "\r\n", ##__VA_ARGS__)
#else
#define user_pid_printf(format, ...)
#define user_pid_info(format, ...)
#define user_pid_debug(format, ...)
#define user_pid_error(format, ...)
#endif


/**
  * @brief  PID参数初始化
	*	@note 	无
  * @retval 无
  */
void PID_param_init()
{
	/* 位置环相关初始化参数 */
    pid_location.target_val=0.0;				
    pid_location.actual_val=0.0;
    pid_location.err=0.0;
    pid_location.err_last=0.0;
    pid_location.integral=0.0;
  
		pid_location.Kp = 0.9;//1.24可以达到31034   1.1+0.01可达31637 
		pid_location.Ki = 0.0005;  //0.9  0.0005 0.0  31741   死区100  期望为31734时 跑一米误差不到1cm
		pid_location.Kd = 0.0;
  
  	/* 电机1速度环相关初始化参数 */
    pid_speed.target_val=0.0;				
    pid_speed.actual_val=0.0;
    pid_speed.err=0.0;
    pid_speed.err_last=0.0;
    pid_speed.integral=0.0;
    pid_speed.setout_val = 0.0;
  
  #ifdef MASTER_MODE              //主机/右轮
		pid_speed.Kp = 36;    //     右轮 35         
		pid_speed.Ki = 0.3;  //        右轮0.3
		pid_speed.Kd = 0.18;   //     右轮0.17      右轮原本0.1比较稳，但是左轮启动更快，所以增大kd

	#else                 //从机/左轮
    pid_speed.Kp = 34.3;  //左轮  34.3           
		pid_speed.Ki = 0.3;   //左轮0.30        
		pid_speed.Kd = 0.16;   //左轮0.16          右轮原本0.1比较稳，但是左轮启动更快，所以增大kd
  #endif
  
  	/* 电机2速度环相关初始化参数 */
    pid_speed2.target_val=0.0;				
    pid_speed2.actual_val=0.0;
    pid_speed2.err=0.0;
    pid_speed2.err_last=0.0;
    pid_speed2.integral=0.0;
  
		pid_speed2.Kp = 0.0;
		pid_speed2.Ki = 0.0;
		pid_speed2.Kd = 0.0;
		
	/* 角度环相关初始化参数 */
	pid_angle.target_val = 0.0;
	pid_angle.actual_val = 0.0;
	pid_angle.err = 0.0;
	pid_angle.err_last = 0.0;
	pid_angle.integral = 0.0;
	
	pid_angle.Kp = 0.0;
	pid_angle.Ki = 0.0;
	pid_angle.Kd = 0.0;
#if defined(PID_ASSISTANT_EN)
    float pid_temp[3] = {pid.Kp, pid.Ki, pid.Kd};   //这个得传入对应的pid结构体才行
    set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp, 3);     // 给通道 1 发送 P I D 值
#endif
}

/**
  * @brief  设置目标值
  * @param  val		目标值
	*	@note 	无
  * @retval 无
  */
void set_pid_target(_pid *pid, float temp_val)
{
  pid->target_val = temp_val;    // 设置当前的目标值
}

/**
  * @brief  获取目标值
  * @param  无
	*	@note 	无
  * @retval 目标值
  */
float get_pid_target(_pid *pid)
{
  return pid->target_val;    // 设置当前的目标值
}

void set_p_i_d(_pid *pid, float p, float i, float d)
{
    pid->Kp = p;    // 设置比例系数 P
		pid->Ki = i;    // 设置积分系数 I
		pid->Kd = d;    // 设置微分系数 D
}

/**
  * @brief  位置PID算法实现
	*	@note 	无
  * @retval 通过PID计算后的输出
  */
void location_pid_realize(_pid *pid)  //位置环光个Kp好像也可以
{
		/*计算目标值与实际值的误差*/
    pid->err=pid->target_val-pid->actual_val;
    // user_pid_debug("auvule = %f,taget = %f",pid->actual_val,pid->target_val);
    // user_pid_debug("err = %f",pid->err);
//    /* 设定闭环死区 */   //外环死区可以不要 
   if((pid->err >= -100) && (pid->err <= 100)) 
   {
     pid->err = 0;
     pid->integral = 0;
   }
    // user_pid_debug("intergral = %f",pid->integral);
    pid->integral += pid->err;    // 误差累积

		/*PID算法实现*/
    pid->setout_val = pid->Kp*pid->err
		                  +pid->Ki*pid->integral
		                  +pid->Kd*(pid->err-pid->err_last);



		/*误差传递*/
    pid->err_last=pid->err;
    
}

void location_pid_realize_nodie(_pid *pid)  //位置环光个Kp好像也可以
{
		/*计算目标值与实际值的误差*/
    pid->err=pid->target_val-pid->actual_val;
    // user_pid_debug("auvule = %f,taget = %f",pid->actual_val,pid->target_val);
    // user_pid_debug("err = %f",pid->err);
//    /* 设定闭环死区 */   //外环死区可以不要 
  //  if((pid->err >= -100) && (pid->err <= 100)) 
  //  {
  //    pid->err = 0;
  //    pid->integral = 0;
  //  }
    // user_pid_debug("intergral = %f",pid->integral);
    pid->integral += pid->err;    // 误差累积

		/*PID算法实现*/
    pid->setout_val = pid->Kp*pid->err
		                  +pid->Ki*pid->integral
		                  +pid->Kd*(pid->err-pid->err_last);



		/*误差传递*/
    pid->err_last=pid->err;
    
}

/**
  * @brief  速度PID算法实现
	*	@note 	无
  * @retval 通过PID计算后的输出
  */
void speed_pid_realize(_pid *pid)  
{
		/*计算目标值与实际值的误差*/
    float x;
    // user_pid_debug("auvule = %f,taget = %f",pid->actual_val,pid->target_val);
    pid->err=pid->target_val - pid->actual_val;
    // user_pid_debug("err = %f",pid->err);
    if((pid->err<0.5f ) && (pid->err>-0.5f))   //假如以最大允许速度偏差运行1分钟，输出轴最大偏差为半圈
		{
      pid->err = 0.0f;
		}
	
		if( fabs(pid->err) < 5)
    {
      pid->integral += pid->err;    // 误差累积
      // user_pid_debug("intergral = %f",pid->integral);
    }
    else {
      pid->integral += 0;
    }
	
	  /*积分限幅*/
	  if (pid->integral >= 97.2f) 
    {
      pid->integral =97.2;
    }
    else if (pid->integral < -97.2)  
    {
      pid->integral = -97.2;
    }

		/*PID算法实现*/
    x = pid->Kp*pid->err
		                  +pid->Ki*pid->integral
		                   +pid->Kd*(pid->err-pid->err_last);

    pid->setout_val = x;
    // user_pid_debug("setou_val = %f",pid->setout_val);
		/*误差传递*/
    pid->err_last=pid->err;
    
		/*返回当前实际值*/
    
}


/*
  角度pid
*/
void angle_pid_realize(_pid *pid)
{
  /*计算目标值与实际值的误差*/
    pid->err= - (pid->target_val-pid->actual_val);
    // if(fabs(pid->target_val-pid->actual_val) >fabs(pid->actual_val-pid->target_val) )   //
    // {
    //   pid->err = pid->actual_val-pid->target_val;
    // }
    // else {
    //   pid->err = pid->target_val-pid->actual_val;
    // }
    user_pid_debug("auvule = %f,taget = %f",pid->actual_val,pid->target_val);
    user_pid_debug("err = %f",pid->err);
//    /* 设定闭环死区 */   //外环死区可以不要 
   if((pid->err >= -2) && (pid->err <= 2)) 
   {
     pid->err = 0;
     pid->integral = 0;
   }
   
    
    pid->integral += pid->err;    // 误差累积
    if (pid->integral > 100) pid->integral = 100;  // 限制积分上限
    if (pid->integral < -100) pid->integral = -100;
		/*PID算法实现*/
    pid->setout_val = pid->Kp*pid->err
		                  +pid->Ki*pid->integral
		                  +pid->Kd*(pid->err-pid->err_last);

    user_pid_debug("output = %f",pid->setout_val);

		/*误差传递*/
    pid->err_last=pid->err;
}
